package utils;

import java.nio.FloatBuffer;

public class Matrix3D {


	public FloatBuffer floatBuffer = null;
	
	
	public void toFloatBuffer4() {
		if (floatBuffer==null)
			floatBuffer = FloatBuffer.allocate(4*4);

		int idx = 0;
		for (int i=0;i<4;i++)
			for (int j=0;j<4;j++) {
				floatBuffer.put(idx, (float)data[i][j]);
				idx++;
			}
	}

	public void toFloatBuffer3() {
		if (floatBuffer==null)
			floatBuffer = FloatBuffer.allocate(3*3);

		int idx = 0;
		for (int i=0;i<3;i++)
			for (int j=0;j<3;j++) {
				floatBuffer.put(idx, (float)data[i][j]);
				idx++;
			}
	}

	
	/** Creates a new instance of Matrix3D with the identity */
	public Matrix3D() {
		data = new double[4][4];
		for (int i = 0; i < 4; i++) {
			data[i][i] = 1;
		}
	}

	/**
	 * Creates a new instance of Matrix3D with a copy of the source
	 * 
	 * @param copy
	 *            Source to copy from
	 */
	public Matrix3D(Matrix3D copy) {
		data = new double[4][4];
		int i, j;
		for (j = 0; j < 4; j++) {
			for (i = 0; i < 4; i++) {
				data[j][i] = copy.data[j][i];
			}
		}
	}

	/**
     * This is Ken Perlin's MatrixInverter
     * http://mrl.nyu.edu/~perlin/courses/fall2004/hw/MatrixInverter.java
     */
    public void invert(Matrix3D result) {

      double[][] src = this.data;
      double[][] dst = result.data;
      
      // COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION/SCALE 3x3 SUBMATRIX
      
      for (int i = 0 ; i < 3 ; i++)
      for (int j = 0 ; j < 3 ; j++) {
         int iu = (i + 1) % 3, iv = (i + 2) % 3;
         int ju = (j + 1) % 3, jv = (j + 2) % 3;
         dst[j][i] = src[iu][ju] * src[iv][jv] - src[iu][jv] * src[iv][ju];
      }

      // RENORMALIZE BY DETERMINANT TO INVERT ROTATION/SCALE SUBMATRIX

      double det = src[0][0]*dst[0][0] + src[1][0]*dst[0][1] + src[2][0]*dst[0][2];
      for (int i = 0 ; i < 3 ; i++)
      for (int j = 0 ; j < 3 ; j++)
         dst[i][j] /= det;

      // INVERT TRANSLATION

      for (int i = 0 ; i < 3 ; i++)
         dst[i][3] = -dst[i][0]*src[0][3] - dst[i][1]*src[1][3] - dst[i][2]*src[2][3];

      // NO PERSPECTIVE

      for (int i = 0 ; i < 4 ; i++)
         dst[3][i] = i < 3 ? 0 : 1;
      
   }
	
	
	
	public void ToTangentSpace(Vec T, Vec B, Vec N) {
		
		data[0][0] = T.x();
		data[0][1] = T.y();
		data[0][2] = T.z();

		data[1][0] = B.x();
		data[1][1] = B.y();
		data[1][2] = B.z();

		data[2][0] = N.x();
		data[2][1] = N.y();
		data[2][2] = N.z();
		
		data[3][3] = 1;

		data[3][0] = 0;
		data[3][1] = 0;
		data[3][2] = 0;

		data[0][3] = 0;
		data[1][3] = 0;
		data[2][3] = 0;
	}
	
	
	/**
	 * Sets an element in the matrix to a value indicies must be between 0-2 -
	 * user beware
	 * 
	 * @param j
	 *            Specifies the row index to insert into
	 * @param i
	 *            Specifies the column index to insert into
	 * @param value
	 *            Value to set to element
	 */
	public void set(int j, int i, double value) {
		data[j][i] = value;
	}

	/**
	 * Gets an element from the matrix indicies must be between 0-2 - user
	 * beware
	 * 
	 * @param j
	 *            Specifies the row index to insert into
	 * @param i
	 *            Specifies the column index to insert into
	 * @return Returns the value contained at the specified location
	 */
	public double get(int j, int i) {
		return data[j][i];
	}

	/**
	 * Transforms a given point using the current matrix
	 * 
	 * @param in
	 *            Specifies a 3D point (4 element array) to transform
	 * @param out
	 *            Specifies a 3D point (4 element array) to transform into
	 */
	public void transform(double[] in, double[] out) {
		int i, k;
		for (k = 0; k < 4; k++) {
			out[k] = 0;
			for (i = 0; i < 4; i++) {
				out[k] += data[k][i] * in[i];
			}
		}
	}

	/**
	 * Multiplies the matrix by the source, such that the source transform
	 * happens before the current transform
	 * 
	 * @param src
	 *            Source transformation matrix
	 */
	public void precompose(Matrix3D src) {
		int i, j, k;
		double results[][] = new double[4][4];
		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				results[i][j] = 0;
				for (k = 0; k < 4; k++) {
					results[i][j] += data[i][k] * src.data[k][j];
				}
			}
		}
		data = results;
	}

	/**
	 * Multiplies the matrix by the source, such that the source transform
	 * happens after the current transform
	 * 
	 * @param src
	 *            Source transformation matrix
	 */
	public void postcompose(Matrix3D src) {
		int i, j, k;
		double results[][] = new double[4][4];
		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				results[i][j] = 0;
				for (k = 0; k < 4; k++) {
					results[i][j] += src.data[i][k] * data[k][j];
				}
			}
		}
		data = results;
	}

	/** Resets the matrix to the identity matrix */
	public void loadIdentity() {
		int j, i;
		for (j = 0; j < 4; j++) {
			for (i = 0; i < 4; i++) {
				data[j][i] = 0;
			}
			data[j][j] = 1;
		}
	}

	/**
	 * Creates a translation matrix to move coordinates
	 * 
	 * @param tx
	 *            Translation amount in the x direction
	 * @param ty
	 *            Translation amount in the y direction
	 * @param tz
	 *            Translation amount in the z direction
	 */
	public void translate(double tx, double ty, double tz) {
		loadIdentity();
		data[0][3] = tx;
		data[1][3] = ty;
		data[2][3] = tz;
	}

	/**
	 * Creates a scaling matrix to scale coordinates
	 * 
	 * @param sx
	 *            Scale amount for x
	 * @param sy
	 *            Scale amount for y
	 * @param sz
	 *            Scale amount for z
	 */
	public void scale(double sx, double sy, double sz) {
		loadIdentity();
		data[0][0] = sx;
		data[1][1] = sy;
		data[2][2] = sz;
	}

	/**
	 * Creates a rotation matrix around an arbitrary axis
	 * 
	 * @param ax
	 *            Axis vector portion in x
	 * @param ay
	 *            Axis vector portion in y
	 * @param az
	 *            Axis vector portion in z
	 * @param angle
	 *            Amount to rotate in radians
	 */
	public void rotate(double ax, double ay, double az, double angle) {
		loadIdentity();
		double maga = Math.sqrt((ax * ax) + (ay * ay) + (az * az));
		double magtcrossw;
		double temptrans[][] = new double[3][3];
		double temptrans2[][] = new double[3][3];
		double trigtrans[][] = new double[3][3];
		double u[] = new double[3];
		double t[] = new double[3];
		double w[] = new double[3];
		double v[] = new double[3];
		double tcrossw[] = new double[3];
		w[0] = t[0] = temptrans[0][2] = ax / maga;
		w[1] = t[1] = temptrans[1][2] = ay / maga;
		w[2] = t[2] = temptrans[2][2] = az / maga;
		temptrans2[2][0] = ax / maga;
		temptrans2[2][1] = ay / maga;
		temptrans2[2][2] = az / maga;
		if (ax < ay && ax < az) {
			t[0] = 1;
		} else if (ay <= ax & ay <= az) {
			t[1] = 1;
		} else {
			t[2] = 1;
		}
		tcrossw[0] = (t[1] * w[2]) - (t[2] * w[1]);
		tcrossw[1] = (t[2] * w[0]) - (t[0] * w[2]);
		tcrossw[2] = (t[0] * w[1]) - (t[1] * w[0]);
		magtcrossw = Math.sqrt((tcrossw[0] * tcrossw[0])
				+ (tcrossw[1] * tcrossw[1]) + (tcrossw[2] * tcrossw[2]));
		u[0] = temptrans[0][0] = tcrossw[0] / magtcrossw;
		u[1] = temptrans[1][0] = tcrossw[1] / magtcrossw;
		u[2] = temptrans[2][0] = tcrossw[2] / magtcrossw;
		temptrans2[0][0] = tcrossw[0] / magtcrossw;
		temptrans2[0][1] = tcrossw[1] / magtcrossw;
		temptrans2[0][2] = tcrossw[2] / magtcrossw;
		v[0] = (w[1] * u[2]) - (w[2] * u[1]);
		v[1] = (w[2] * u[0]) - (w[0] * u[2]);
		v[2] = (w[0] * u[1]) - (w[1] * u[0]);
		temptrans[0][1] = v[0];
		temptrans[1][1] = v[1];
		temptrans[2][1] = v[2];
		temptrans2[1][0] = v[0];
		temptrans2[1][1] = v[1];
		temptrans2[1][2] = v[2];

		trigtrans[0][0] = Math.cos(angle);
		trigtrans[0][1] = -Math.sin(angle);
		trigtrans[1][0] = Math.sin(angle);
		trigtrans[1][1] = Math.cos(angle);
		trigtrans[2][2] = 1;

		int i, j, k;
		double results[][] = new double[3][3];
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				results[i][j] = 0;
				for (k = 0; k < 3; k++) {
					results[i][j] += temptrans[i][k] * trigtrans[k][j];
				}
			}
		}
		temptrans = results;
		results = new double[3][3];
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				results[i][j] = 0;
				for (k = 0; k < 3; k++) {
					results[i][j] += temptrans[i][k] * temptrans2[k][j];
				}
			}
		}
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				data[i][j] = results[i][j];
			}
		}
		data[3][3] = 1;
	}

	/**
	 * Creates a look at transformation matrix for arbitrary view positions
	 * 
	 * @param eyex
	 *            eye position in x
	 * @param eyey
	 *            eye position in y
	 * @param eyez
	 *            eye position in z
	 * @param atx
	 *            look at position in x
	 * @param aty
	 *            look at position in y
	 * @param atz
	 *            look at position in z
	 * @param upx
	 *            up direction in x
	 * @param upy
	 *            up direction in y
	 * @param upz
	 *            up direction in z
	 */
	public void lookAt(double eyex, double eyey, double eyez, double atx,
			double aty, double atz, double upx, double upy, double upz) {
		double w[] = new double[3];
		double upxw[] = new double[3];
		double u[] = new double[3];
		double matrix1[][] = new double[4][4];
		double matrix2[][] = new double[4][4];
		double wmag = Math.sqrt((atx * atx) + (aty * aty) + (atz * atz));
		w[0] = -atx / wmag;
		w[1] = -aty / wmag;
		w[2] = -atz / wmag;
		upxw[0] = (upy * w[2]) - (upz * w[1]);
		upxw[1] = (upy * w[0]) - (upx * w[2]);
		upxw[2] = (upx * w[1]) - (upy * w[0]);
		double upxwmag = Math.sqrt((upxw[0] * upxw[0]) + (upxw[1] * upxw[1])
				+ (upxw[2] * upxw[2]));
		u[0] = upxw[0] / upxwmag;
		u[1] = upxw[1] / upxwmag;
		u[2] = upxw[2] / upxwmag;
		matrix1[0][0] = u[0];
		matrix1[0][1] = u[1];
		matrix1[0][2] = u[2];
		matrix1[1][0] = (w[1] * u[2]) - (w[2] * u[1]);
		matrix1[1][1] = (w[1] * u[0]) - (w[0] * u[2]);
		matrix1[1][2] = (w[0] * u[1]) - (w[1] * u[0]);
		matrix1[2][0] = w[0];
		matrix1[2][1] = w[1];
		matrix1[2][2] = w[2];
		matrix1[3][3] = 1;
		matrix2[0][0] = 1;
		matrix2[1][1] = 1;
		matrix2[2][2] = 1;
		matrix2[3][3] = 1;
		matrix2[0][3] = -eyex;
		matrix2[1][3] = -eyey;
		matrix2[2][3] = -eyez;
		int i, j, k;
		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				data[i][j] = 0;
				for (k = 0; k < 4; k++) {
					data[i][j] += matrix1[i][k] * matrix2[k][j];
				}
			}
		}
	}

	/**
	 * Creates a matrix to transform coordinates into a normal viewing
	 * perspective
	 * 
	 * @param left
	 *            left position of the view
	 * @param right
	 *            right position of the view
	 * @param bottom
	 *            bottom position of the view
	 * @param top
	 *            top position of the view
	 * @param near
	 *            nearest position of the view
	 * @param far
	 *            furthest position of the view
	 */
	public void perspective(double left, double right, double bottom,
			double top, double near, double far) {
		Matrix3D temp = new Matrix3D();
		temp.orthographic(left, right, bottom, top, near, far);
		data[0][0] = 1;
		data[0][1] = 0;
		data[0][2] = 0;
		data[0][3] = 0;
		data[1][0] = 0;
		data[1][1] = 1;
		data[1][2] = 0;
		data[1][3] = 0;
		data[2][0] = 0;
		data[2][1] = 0;
		data[2][2] = (near + far) / near;
		data[2][3] = -far;
		data[3][0] = 0;
		data[3][1] = 0;
		data[3][2] = 1 / near;
		data[3][3] = 0;
		precompose(temp);
	}

	/**
	 * Creates a matrix to transform coordinates into more of an engineering
	 * view
	 * 
	 * @param left
	 *            left position of the view
	 * @param right
	 *            right position of the view
	 * @param bottom
	 *            bottom position of the view
	 * @param top
	 *            top position of the view
	 * @param near
	 *            nearest position of the view
	 * @param far
	 *            furthest position of the view
	 */
	public void orthographic(double left, double right, double bottom,
			double top, double near, double far) {
		loadIdentity();
		double matrix1[][] = new double[4][4];
		double matrix2[][] = new double[4][4];
		matrix1[0][0] = 2 / (right - left);
		matrix1[1][1] = 2 / (top - bottom);
		matrix1[2][2] = 2 / (near - far);
		matrix1[3][3] = 1;
		matrix2[0][0] = 1;
		matrix2[1][1] = 1;
		matrix2[2][2] = 1;
		matrix2[3][3] = 1;
		matrix2[0][3] = -(left + right) / 2;
		matrix2[1][3] = -(bottom + top) / 2;
		matrix2[2][3] = -(near + far) / 2;
		int i, j, k;
		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				data[i][j] = 0;
				for (k = 0; k < 4; k++) {
					data[i][j] += matrix1[i][k] * matrix2[k][j];
				}
			}
		}
	}

	/** Used to store the 4x4 matrix */
	private double data[][];
}
